/*
* Copyright (C) Microsoft. All rights reserved.  
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not 
* use this file except in compliance with the License.  You may obtain a copy 
* of the License at http://www.apache.org/licenses/LICENSE-2.0  
* 
* THIS CODE IS PROVIDED *AS IS* BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
* KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION ANY IMPLIED 
* WARRANTIES OR CONDITIONS OF TITLE, FITNESS FOR A PARTICULAR PURPOSE, 
* MERCHANTABLITY OR NON-INFRINGEMENT. 
*
* See the Apache License, Version 2.0 for specific language governing 
* permissions and limitations under the License.
*
*/
#pragma once

#include <math.h>
#include "baja/common.hpp"
#include "baja/assert.hpp"
#include "baja/math_fwd.hpp"
#include "baja/math_classes.hpp"

namespace baja { namespace math {

    inline float fastSqrt(float x)
    {
        union { int i; float x; } u;
        u.x = x; 
        u.i = (1<<29) + (u.i >> 1) - (1<<22); 
        u.x = 0.5f * (u.x + x/u.x);
        return u.x;
    }

    // point2d

    BAJA_INLINE point2d::point2d(const vector2d& other)
    { 
        this->x = other.x; 
        this->y = other.y; 
    }

    BAJA_INLINE bool point2d::operator==(const point2d& other) const 
    { 
        return this->x == other.x && this->y == other.y; 
    }

    BAJA_INLINE bool point2d::operator!=(const point2d& other) const 
    { 
        return this->x != other.x || this->y != other.y; 
    }

    BAJA_INLINE point2d& point2d::operator=(const point2d& other)
    {
        this->x = other.x;
        this->y = other.y;
        return *this;
    }

    BAJA_INLINE void point2d::set(const float32 x, const float32 y) 
    { 
        this->x = x; 
        this->y = y; 
    }

    BAJA_INLINE void point2d::offsetBy(const vector2d& vector) 
    { 
        this->x += vector.x; 
        this->y += vector.y; 
    }

    BAJA_INLINE void point2d::offsetBy(const vector2d& vector, point2d* result) const 
    { 
        result->x = this->x + vector.x; 
        result->y = this->y + vector.y; 
    }

    BAJA_INLINE void point2d::vectorTo(const point2d& point, vector2d* result) const 
    { 
        result->x = point.x - this->x; 
        result->y = point.y - this->y; 
    }

    BAJA_INLINE void point2d::scale(float32 scale) 
    { 
        this->x *= scale; 
        this->y *= scale; 
    }

    BAJA_INLINE void point2d::scale(float32 scale, point2d* result) 
    { 
        result->x = this->x * scale; 
        result->y = this->y * scale; 
    }

    BAJA_INLINE void point2d::lerpTo(const point2d& point, float32 t, point2d* result) const
    {
        result->x = this->x + (t * (point.x - this->x));
        result->y = this->y + (t * (point.y - this->y));
    }

    BAJA_INLINE vector2d::vector2d(const point2d& point) : x(point.x), y(point.y) {} 

    BAJA_INLINE bool vector2d::operator==(const vector2d& other) const 
    { 
        return this->x == other.x && this->y == other.y; 
    }

    BAJA_INLINE bool vector2d::operator!=(const vector2d& other) const 
    { 
        return this->x != other.x || this->y != other.y; 
    }

    BAJA_INLINE vector2d& vector2d::operator=(const vector2d& other)
    {
        this->x = other.x;
        this->y = other.y;
        return *this;
    }

    BAJA_INLINE vector2d& vector2d::operator+=(const vector2d& that) 
    { 
        this->x += that.x; 
        this->y += that.y; 
        return *this; 
    }

    BAJA_INLINE vector2d& vector2d::operator-=(const vector2d& that) 
    { 
        this->x -= that.x; 
        this->y -= that.y; 
        return *this; 
    }

    BAJA_INLINE void vector2d::set(float32 x, float32 y) 
    { 
        this->x = x; 
        this->y = y; 
    }

    BAJA_INLINE void vector2d::add(const vector2d& that, vector2d* result) const 
    { 
        result->x = this->x + that.x; 
        result->y = this->y + that.y; 
    }

    BAJA_INLINE void vector2d::subtract(const vector2d& that, vector2d* result) const 
    { 
        result->x = this->x - that.x; 
        result->y = this->y - that.y; 
    }

    BAJA_INLINE void vector2d::scale(const float32 scale) 
    { 
        this->x *= scale; 
        this->y *= scale; 
    }

    BAJA_INLINE void vector2d::scale(const float32 scale, vector2d* result) const 
    { 
        result->x = this->x * scale; 
        result->y = this->y * scale; 
    }

    BAJA_INLINE float32 vector2d::lengthSquared() const 
    { 
        return (this->x * this->x) + (this->y * this->y); 
    }

    BAJA_INLINE float32 vector2d::length() const 
    { 
        return sqrtf((this->x * this->x) + (this->y * this->y)); 
    }

    BAJA_INLINE float32 vector2d::dot(const vector2d& that) const 
    { 
        return (this->x * that.x) + (this->y * that.y); 
    }

    BAJA_INLINE void vector2d::cross(vector2d* result) const 
    { 
        result->x = this->y; 
        result->y = -this->x; 
    }

    BAJA_INLINE float32 vector2d::angleBetween(const vector2d other) const
    {
        float32 thisLen = this->length();
        float32 otherLen = other.length();

        float32 thedot = this->dot(other) / (thisLen * otherLen);
        thedot = min(1, max(0, thedot));
        return acosf(thedot);
    }

    BAJA_INLINE void vector2d::lerpTo(const vector2d& v, float32 t, vector2d* result) const
    {
        result->x = this->x + (t * (v.x - this->x));
        result->y = this->y + (t * (v.y - this->y));
    }

    BAJA_INLINE void vector2d::normalize()
    {
        float32 len = this->length();
        float32 lenInv = (len != 0.0f ? 1.0f / len : 0.0f);

        this->x *= lenInv;
        this->y *= lenInv;
    }

    BAJA_INLINE void vector2d::normalize(vector2d* result) const
    {
        float32 len = this->length();
        float32 lenInv = (len != 0.0f ? 1.0f / len : 0.0f);

        result->x = this->x * lenInv;
        result->y = this->y * lenInv;
    }

    // rectangle
    BAJA_INLINE rectangle::rectangle(float32 l, float32 t, float32 r, float32 b)
    { 
        this->set(l,t,r,b); 
    }

    BAJA_INLINE void rectangle::set(float32 l, float32 t, float32 r, float32 b)
    { 
        this->left = l;   
        this->top = t;   
        this->right = r;   
        this->bottom = b; 
    }

    BAJA_INLINE void rectangle::translate(const vector2d& translationVector)
    { 
        this->left += translationVector.x;
        this->right += translationVector.x;
        this->top += translationVector.y;
        this->bottom += translationVector.y; 
    }

    BAJA_INLINE bool rectangle::intersects(const rectangle& other) const
    { 
        return 
            this->right >= other.left && this->left <= other.right &&
            this->top <= other.bottom && this->bottom >= other.top; 
    }

    template <typename T>
    BAJA_INLINE void rectangle::getSize(T* width, T* height) const
    { 
        BAJA_VERIFY(width && height);
        *width = (T)(this->right - this->left);
        *height = (T)(this->bottom - this->top); 
    }

    template <typename T>
    BAJA_INLINE void rectangle::getCenter(T* x, T* y) const
    { 
        BAJA_VERIFY(x && y);

        *x = (T)(left + (this->right - this->left) * 0.5f);
        *y = (T)(top + (this->bottom - this->top) * 0.5f); 
    }

    BAJA_INLINE void rectangle::forceIntoBounds(const rectangle& bounds)
    {
        if (this->right > bounds.right)
        {
            float32 delta = this->right - bounds.right;
            this->right -= delta;
            this->left -= delta;
        }

        if (this->left < bounds.left)
        {
            float32 delta = this->left - bounds.left;
            this->right = delta;
            this->left -= delta;
        }

        if (this->bottom > bounds.bottom)
        {
            float32 delta = this->bottom - bounds.bottom;
            this->bottom -= delta;
            this->top -= delta;
        }

        if (this->top < bounds.top)
        {
            float32 delta = this->top - bounds.top;
            this->top -= delta;
            this->bottom -= delta;
        }
    }

    // point3d

    BAJA_INLINE point3d::point3d(const vector3d& vector) : x(vector.x), y(vector.y), z(vector.z) {}

    BAJA_INLINE bool point3d::operator==(const point3d& that) const 
    { 
        return (this->x == that.x && this->y == that.y && this->z == that.z); 
    }

    BAJA_INLINE bool point3d::operator!=(const point3d& that) const 
    { 
        return (this->x != that.x || this->y != that.y || this->z != that.z); 
    }

    BAJA_INLINE point3d& point3d::operator=(const point3d& that) 
    { 
        this->x = that.x; 
        this->y = that.y; 
        this->z = that.z; 
        return *this; 
    }

    BAJA_INLINE void point3d::set(const float32 x, const float32 y, const float32 z) 
    { 
        this->x = x; 
        this->y = y; 
        this->z = z; 
    }

    BAJA_INLINE void point3d::offsetBy(const vector3d& v) 
    { 
        this->x += v.x; 
        this->y += v.y;  
        this->z += v.z; 
    }

    BAJA_INLINE void point3d::offsetBy(const vector3d& v, point3d* result) const 
    { 
        result->x = this->x + v.x; 
        result->y = this->y + v.y;  
        result->z = this->z + v.z; 
    }

    BAJA_INLINE void point3d::vectorTo(const point3d& p, vector3d* result) const 
    { 
        result->x = p.x - this->x; 
        result->y = p.y - this->y; 
        result->z = p.z - this->z; 
    }

    BAJA_INLINE void point3d::scale(const float32 scale) 
    { 
        this->x *= scale; 
        this->y *= scale; 
        this->z *= scale; 
    }

    BAJA_INLINE void point3d::scale(const float32 scale, point3d* result) 
    { 
        result->x = this->x * scale; 
        result->y = this->y * scale; 
        result->z = this->z * scale; 
    }

    BAJA_INLINE void point3d::lerpTo(const point3d& p, const float32 t, point3d* result) const
    {
        result->x = this->x + (t * (p.x - this->x));
        result->y = this->y + (t * (p.y - this->y));
        result->z = this->z + (t * (p.z - this->z));
    }

    BAJA_INLINE float32 point3d::distanceToLine(const line3d& line) const
    {
        vector3d lineDirection(line.direction);
        float32 lineLengthSq = lineDirection.lengthSquared();
        if (lineLengthSq <= c::floatEpsilon)
        {
            // line has no length....
            return FLT_MAX;
        }

        line3d l(line.point, line.direction);

        point3d p1 = line.point;
        point3d p2;
        l.getPointAt(1.0f, &p2);

        float32 u = (((x - p1.x) * (p2.x - p1.x)) + ((y - p1.y) * (p2.y - p1.y))) / lineLengthSq;

        point3d p3;
        l.getPointAt(u, &p3);

        vector3d v;
        this->vectorTo(p3, &v);

        return v.length();
    }

    // vector3d 

    BAJA_INLINE vector3d::vector3d(const point3d& point)
    { 
        this->x = point.x;
        this->y = point.y;
        this->z = point.z; 
    }

    BAJA_INLINE bool vector3d::operator==(const vector3d& that) const
    { 
        return (this->x == that.x && this->y == that.y && this->z == that.z); 
    }

    BAJA_INLINE bool vector3d::operator!=(const vector3d& that) const
    { 
        return (this->x != that.x || this->y != that.y || this->z != that.z); 
    }

    BAJA_INLINE vector3d& vector3d::operator=(const vector3d& that)
    { 
        this->x = that.x;
        this->y = that.y;
        this->z = that.z;
        return *this; 
    }

    BAJA_INLINE vector3d& vector3d::operator+=(const vector3d& that)
    { 
        this->x += that.x;
        this->y += that.y;
        this->z += that.z;
        return *this; 
    }

    BAJA_INLINE vector3d& vector3d::operator-=(const vector3d& that)
    { 
        this->x -= that.x;
        this->y -= that.y;
        this->z -= that.z;
        return *this; 
    }

    BAJA_INLINE void vector3d::set(const float32 x, const float32 y, const float32 z)
    { 
        this->x = x;
        this->y = y;
        this->z = z; 
    }

    BAJA_INLINE void vector3d::add(const vector3d& that, vector3d* result) const
    { 
        result->x = this->x + that.x;
        result->y = this->y + that.y; 
        result->z = this->z + that.z; 
    }

    BAJA_INLINE void vector3d::subtract(const vector3d& that, vector3d* result) const
    { 
        result->x = this->x - that.x;
        result->y = this->y - that.y;
        result->z = this->z - that.z; 
    }

    BAJA_INLINE void vector3d::scale(const float32 scale)
    { 
        this->x *= scale;
        this->y *= scale; 
        this->z *= scale; 
    }

    BAJA_INLINE void vector3d::scale(const float32 scale, vector3d* result) const
    { 
        result->x = this->x * scale;
        result->y = this->y * scale;
        result->z = this->z * scale; 
    }

    BAJA_INLINE float32 vector3d::angleBetween(const vector3d other) const
    {
        float32 thisLen = this->length();
        float32 otherLen = ((vector3d*)&other)->length();

        float32 thedot = this->dot(other) / (thisLen * otherLen);
        thedot = min(1, max(0, thedot));
        return acosf(thedot);
    }

    BAJA_INLINE float32 vector3d::angleBetweenNormalized(const vector3d other) const
    {
        float32 thedot = this->dot(other);
        thedot = min(1, max(0, thedot));
        return acosf(thedot);
    }

    BAJA_INLINE float32 vector3d::lengthSquared() const
    { 
        return (this->x * this->x) + (this->y * this->y) + (this->z * this->z); 
    }

    BAJA_INLINE float32 vector3d::length() const
    { 
        return sqrtf((this->x * this->x) + (this->y * this->y) + (this->z * this->z)); 
    }

    BAJA_INLINE float32 vector3d::dot(const vector3d& that) const
    { 
        return (this->x * that.x) + (this->y * that.y) + (this->z * that.z); 
    }

    BAJA_INLINE void vector3d::cross(const vector3d& that, vector3d* result) const
    { 
        result->x = (this->y * that.z) - (this->z * that.y);
        result->y = (this->z * that.x) - (this->x * that.z);
        result->z = (this->x * that.y) - (this->y * that.x); 
    }

    BAJA_INLINE void vector3d::lerpTo(const vector3d& v, const float32 t, vector3d* result) const
    {
        result->x = this->x + (t * (v.x - this->x));
        result->y = this->y + (t * (v.y - this->y));
        result->z = this->z + (t * (v.z - this->z));
    }

    BAJA_INLINE void vector3d::normalize()
    {
        float32 len = this->length();
        float32 lenInv = (len != 0.0f ? 1.0f / len : 0.0f);

        this->x *= lenInv;
        this->y *= lenInv;
        this->z *= lenInv;
    }

    BAJA_INLINE void vector3d::normalize(vector3d* result) const
    {
        float32 len = this->length();
        float32 lenInv = (len != 0.0f ? 1.0f / len : 0.0f);

        result->x = this->x * lenInv;
        result->y = this->y * lenInv;
        result->z = this->z * lenInv;
    }

    // line3d

    BAJA_INLINE line3d::line3d(const point3d& point, const vector3d& direction)
    { 
        this->set(point, direction); 
    }

    BAJA_INLINE line3d::line3d(const point3d& start, const point3d& end)
    { 
        this->set(start, end); 
    }

    BAJA_INLINE void line3d::set(const point3d& point, const vector3d& direction)
    { 
        this->point = point;
        this->direction = direction; 
    }

    BAJA_INLINE void line3d::set(const point3d& start, const point3d& end)
    { 
        this->point = start;
        ((point3d)start).vectorTo(end, &this->direction); 
    }

    BAJA_INLINE void line3d::getPointAt(const float32 t, point3d* result) const
    { 
        result->x = this->point.x + (t * this->direction.x);
        result->y = this->point.y + (t * this->direction.y);
        result->z = this->point.z + (t * this->direction.z); 
    }

    BAJA_INLINE bool line3d::intersects(const plane3d& plane, float32* intersectionT) const
    {
        float32 denominator = ((vector3d)plane.normal).dot(this->direction);

        // if denominator is 0 then line and plane are parallel
        if (denominator >= -c::floatEpsilon && denominator <= c::floatEpsilon)
        {
            return false;
        }

        // otherwise, line does intersect the plane. 
        vector3d lineToPlane;
        ((point3d)this->point).vectorTo(plane.point, &lineToPlane);

        // compute the intersecting T value.
        *intersectionT = ((vector3d)plane.normal).dot(lineToPlane) / denominator;

        return true;
    }

    BAJA_INLINE bool line3d::intersectsAABB(
        const vector3d& boxMin,
        const vector3d& boxMax,
        float32* outIntersectionT
        ) const
    {
        BAJA_VERIFY(outIntersectionT);

        vector3d scale(
            (boxMax.x - boxMin.x),
            (boxMax.y - boxMin.y),
            (boxMax.z - boxMin.z)
            );

        point3d invTrans(
            -(scale.x * 0.5f + boxMin.x),
            -(scale.y * 0.5f + boxMin.y),
            -(scale.z * 0.5f + boxMin.z)
            );

        matrix44 inv;
        inv.setTranslation(invTrans);
        inv.scale(
            1/scale.x,
            1/scale.y,
            1/scale.z
            );

        static const plane3d planes[] = 
        {
            plane3d(point3d(0.5f, 0, 0), vector3d(1, 0, 0)),
            plane3d(point3d(-0.5f, 0, 0), vector3d(-1, 0, 0)),
            plane3d(point3d(0, 0.5f, 0), vector3d(0, 1, 0)),
            plane3d(point3d(0, -0.5f, 0), vector3d(0, -1, 0)),
            plane3d(point3d(0, 0, 0.5f), vector3d(0, 0, 1)),
            plane3d(point3d(0, 0, -0.5f), vector3d(0, 0, -1)),
        };

        float32 minT = FLT_MAX;
        bool hit = false;

        line3d localLine;
        inv.apply(this->direction, &localLine.direction);
        inv.apply(this->point, &localLine.point);

        // 6 aabb sides
        for (uint32 i = 0; i < 6; i++)
        {
            float32 t = 0;
            if (localLine.intersects(planes[i], &t) && t < minT)
            {
                point3d localHitPos;
                localLine.getPointAt(t, &localHitPos);

                localHitPos.x -= planes[i].point.x;
                localHitPos.y -= planes[i].point.y;
                localHitPos.z -= planes[i].point.z;

                if (localHitPos.x >= -0.5f && localHitPos.x <= 0.5f &&
                    localHitPos.y >= -0.5f && localHitPos.y <= 0.5f &&
                    localHitPos.z >= -0.5f && localHitPos.z <= 0.5f)
                {
                    minT = t;
                    hit = true;
                }
            }
        }

        *outIntersectionT = minT;
        return hit;
    }

    BAJA_INLINE bool line3d::intersectsTriangle(
        const point3d& v0,
        const point3d& v1,
        const point3d& v2,
        float32* outIntersectionT
        ) const
    {
        vector3d e0;
        ((point3d*)&v0)->vectorTo(v1, &e0);

        vector3d e1;
        ((point3d*)&v0)->vectorTo(v2, &e1);

        vector3d e0Norm;
        vector3d e1Norm;
        e0.normalize(&e0Norm);
        e1.normalize(&e1Norm);

        // the length of plane normal doesn't matter as long as
        // it's non-zero, so use the normalized e vectors to avoid rounding errors
        vector3d planeNormal;
        e0Norm.cross(e1Norm, &planeNormal);
        if (planeNormal.length() < c::floatEpsilon)
        {
            // we don't handle degenerate triangles
            return false;
        }

        // the length of this normal does matter (used in the cross product below)
        // so don't use e vectors as computed
        vector3d normal;
        e0.cross(e1, &normal);

        plane3d plane(v0, planeNormal);

        float32 t = 0;
        if (this->intersects(plane, &t))
        {
            point3d intersectionPoint;
            this->getPointAt(t, &intersectionPoint);

            vector3d Q;
            ((point3d*)&v0)->vectorTo(intersectionPoint, &Q);

            float32 e00 = e0.dot(e0);
            float32 e01 = e0.dot(e1);
            float32 e11 = e1.dot(e1);

            float32 q0 = e0.dot(Q);
            float32 q1 = e1.dot(Q);

            float32 s0 = (e11 * q0 - e01 * q1);
            if (s0 >= 0)
            {
                float32 s1 = (e00 * q1 - e01 * q0);
                if (s1 >= 0)
                {
                    float32 delta = normal.lengthSquared();
                    if (s0 + s1 <= delta)
                    {
                        *outIntersectionT = t;
                        return true;
                    }
                }
            }
        }

        return false;
    }

    BAJA_INLINE bool line3d::intersectsSphere(
        const point3d& spherePosition,
        float32 sphereRadius,
        float32* outIntersectionT
        ) const
    {
        BAJA_VERIFY(outIntersectionT);

        vector3d toRay;

        point3d* spherePos = ((point3d*)&spherePosition);
        spherePos->vectorTo(
            this->point, 
            &toRay
            );

        vector3d* dirVec = ((vector3d*)&this->direction);
        vector3d normalizedDirection(this->direction);
        normalizedDirection.normalize();

        float32 lenScale = 1 / dirVec->length();

        float32 b = toRay.dot(normalizedDirection);

        float32 c = toRay.lengthSquared() - (sphereRadius * sphereRadius);
        float32 d = b * b - c;

        if (d > 0)
        {
            float32 t = lenScale * (-b - sqrt(d));
            *outIntersectionT = t;
            return true;
        }

        *outIntersectionT = FLT_MAX;
        return false;
    }

    BAJA_INLINE float32 lineHelper(point3d* points, uint32 a, uint32 b, uint32 c, uint32 d)
    {
        return 
            (points[a].x-points[b].x) * (points[c].x-points[d].x) + 
            (points[a].y-points[b].y) * (points[c].y-points[d].y) + 
            (points[a].z-points[b].z) * (points[c].z-points[d].z);
    }

    // computes the shortest line segment between line segements (where t in [0, 1] along lines)
    // returns t1 and t2 which are parameters which are parameters along the 2 input lines which 
    // give the 2 points functioning as endpoints of the shortest line segment
    // returns false if lines are parallel
    BAJA_INLINE bool line3d::shortestLineBetween(const line3d line, float32* outT1, float32* outT2)
    {
        BAJA_VERIFY(outT1);
        BAJA_VERIFY(outT2);

        // get the end points of the 2 line segements we're testing
        point3d points[4];
        points[0] = this->point;
        this->getPointAt(1, &points[1]);
        points[2] = line.point;
        ((line3d*)&line)->getPointAt(1, &points[3]);

        float32 d4343 = lineHelper(points, 3, 2, 3, 2);
        float32 d4321 = lineHelper(points, 3, 2, 1, 0);
        float32 d2121 = lineHelper(points, 1, 0, 1, 0);
        float32 d1343 = lineHelper(points, 0, 2, 3, 2);
        float32 d1321 = lineHelper(points, 0, 2, 1, 0);

        float32 denom = d2121 * d4343 - d4321 * d4321;
        if (fabsf(d4343) < FLT_EPSILON || fabsf(denom) < FLT_EPSILON)
        {
            return false;
        }

        float32 t1 = (d1343 * d4321 - d1321 * d4343) / denom;
        float32 t2 = (d1343 + t1 * d4321) / d4343;

        *outT1 = t1;
        *outT2 = t2;

        return true;
    }

    // plane3d

    BAJA_INLINE plane3d::plane3d(const point3d& point, const vector3d& normal)
    { 
        this->set(point, normal); 
    }

    BAJA_INLINE plane3d::plane3d(const point4d& abcd)
    {
        vector3d n(abcd.x, abcd.y, abcd.z);
        float32 D = abcd.w;
        float32 lenN = n.length();
        BAJA_VERIFY(lenN != 0.0f);

        point3d pos;
        if (lenN != 0.0f)
        {
            vector3d normalizedN;
            n.normalize(&normalizedN);
            normalizedN.scale(-D/lenN, (vector3d*)&pos);
        }
        this->set(pos, n); 
    }

    BAJA_INLINE void plane3d::set(const point3d& point, const vector3d& normal)
    { 
        this->point = point;
        this->normal = normal; 
    }

    BAJA_INLINE void plane3d::project(const vector3d& vec, vector3d* outVec) const
    {
        vector3d n = this->normal;
        n.normalize();

        // project the vector onto the plane normal
        n.scale(n.dot(vec));

        // subtract from original
        vec.subtract(n, outVec);
    }

    BAJA_INLINE void plane3d::project(const point3d& pos, point3d* outPoint) const
    {
        point3d pt = this->point;
        vector3d toPos;
        pt.vectorTo(pos, &toPos);

        vector3d toPointProjOnNorm = this->normal;
        toPointProjOnNorm.normalize();

        // project the vector onto the plane normal
        toPointProjOnNorm.scale(-toPointProjOnNorm.dot(toPos));

        point3d pointToProject(pos);
        pointToProject.offsetBy(toPointProjOnNorm, outPoint);
    }

    BAJA_INLINE bool plane3d::isPointAbovePlane(const point3d& p)
    {
        vector3d n = this->normal;
        n.normalize();

        vector3d v;
        this->point.vectorTo(p, &v);
        v.normalize();

        return v.dot(n) > -c::floatEpsilon;
    }

    BAJA_INLINE bool plane3d::isPointOnOrAbovePlane(const point3d& p)
    {
        vector3d n = this->normal;
        n.normalize();

        vector3d v;
        this->point.vectorTo(p, &v);
        v.normalize();

        return v.dot(n) >= -c::onOrAboveEpsilon;
    }

    BAJA_INLINE bool plane3d::isPointBelowPlane(const point3d& p)
    {
        vector3d n = this->normal;
        n.normalize();

        vector3d v;
        this->point.vectorTo(p, &v);
        v.normalize();

        return v.dot(n) < c::floatEpsilon;
    }

    BAJA_INLINE bool plane3d::isPointOnOrBelowPlane(const point3d& p)
    {
        vector3d n = this->normal;
        n.normalize();

        vector3d v;
        this->point.vectorTo(p, &v);
        v.normalize();

        return v.dot(n) <= c::onOrAboveEpsilon;
    }

    BAJA_INLINE bool plane3d::isPointOnPlane(const point3d& p)
    {
        vector3d n = this->normal;
        n.normalize();

        vector3d v;
        this->point.vectorTo(p, &v);
        v.normalize();

        float32 d = v.dot(n);
        return (d >= -c::floatEpsilon && d <= c::floatEpsilon);
    }

    // point4d

    BAJA_INLINE point4d::point4d(const point4d& point)
    { 
        this->x = point.x;
        this->y = point.y; 
        this->z = point.z;
        this->w = point.w; 
    }

    BAJA_INLINE point4d::point4d(const point3d& point, float32 w)
    { 
        this->x = point.x;
        this->y = point.y; 
        this->z = point.z;
        this->w = w; 
    }

    BAJA_INLINE point4d& point4d::operator=(const point2d& that)
    { 
        this->x = that.x;
        this->y = that.y;
        this->z = 0.0f;
        this->w = 1.0f;
        return *this; 
    }

    BAJA_INLINE point4d& point4d::operator=(const point3d& that)
    { 
        this->x = that.x;
        this->y = that.y;
        this->z = that.z;
        this->w = 1.0f;
        return *this; 
    }

    BAJA_INLINE point4d& point4d::operator=(const point4d& that)
    { 
        this->x = that.x;
        this->y = that.y;
        this->z = that.z;
        this->w = that.w;
        return *this; 
    }

    BAJA_INLINE point4d point4d::add(const point4d& that) const
    {
        return point4d(
            this->x + that.x, 
            this->y + that.y, 
            this->z + that.z, 
            this->w + that.w
            );
    }

    BAJA_INLINE point4d point4d::subtract(const point4d& that) const
    {
        return point4d(
            this->x - that.x, 
            this->y - that.y, 
            this->z - that.z, 
            this->w - that.w
            );
    }

    BAJA_INLINE void point4d::set(float32 x, float32 y, float32 z)
    { 
        this->x = x;
        this->y = y;
        this->z = z; 
    }

    // quaternion

    BAJA_INLINE quaternion::quaternion(const matrix33& matrix)
    { 
        this->fromMatrix(matrix); 
    }

    BAJA_INLINE quaternion::quaternion(const matrix44& matrix)
    { 
        this->fromMatrix(matrix); 
    }

    BAJA_INLINE quaternion& quaternion::operator=(const quaternion& that)
    { 
        this->x = that.x;
        this->y = that.y;
        this->z = that.z;
        this->w = that.w;
        return *this; 
    }

    BAJA_INLINE void quaternion::normalize()
    {
        float32 magnitudeSq = (this->x * this->x) + (this->y * this->y) + (this->z * this->z) + (this->w * this->w);
        float32 magnitude = sqrtf(magnitudeSq);

        if (magnitude != 0.0f)
        {
            float32 magnitudeInverse = 1.0f / magnitude;

            this->x *= magnitudeInverse;
            this->y *= magnitudeInverse;
            this->z *= magnitudeInverse;
            this->w *= magnitudeInverse;
        }
        else
        {
            // invalid quaternion, reset to valid values
            this->x = 0.0f;
            this->y = 0.0f;
            this->z = 0.0f;
            this->w = 1.0f;
        }
    }

    // slerp this quaternion into the destination quaternion specified, based on the t value.
    BAJA_INLINE void quaternion::slerp(const quaternion& dest, float32 t, quaternion* result) const
    {
        float64 cosom;
        float64 scale0; 
        float64 scale1;
        float32 to1[4];

        // get cosine
        cosom = this->x * dest.x + this->y * dest.y + this->z * dest.z + this->w * dest.w;

        // fixup sign
        if (cosom < 0.0)
        {
            cosom = -cosom;
            to1[0] = -dest.x;
            to1[1] = -dest.y;
            to1[2] = -dest.z;
            to1[3] = -dest.w;
        }
        else
        {
            to1[0] = dest.x;
            to1[1] = dest.y;
            to1[2] = dest.z;
            to1[3] = dest.w;
        }

        // compute coefficients
        if (1.0 - cosom > c::floatEpsilon)
        {
            // quaternions are far enough apart that slerp is right thing to do
            float64 omega;
            float64 sinOmega;

            omega = acos(cosom);
            sinOmega = sin(omega);

            scale0 = sin((1.0 - t) * omega) / sinOmega;
            scale1 = sin(t * omega) / sinOmega;
        }
        else
        {
            // quaternions are too close together, so lerp works best
            scale0 = 1.0 - t;
            scale1 = 1.0;
        }

        // compute final values
        result->x = (float32)((scale0 * this->x) + (scale1 * to1[0]));
        result->y = (float32)((scale0 * this->y) + (scale1 * to1[1]));
        result->z = (float32)((scale0 * this->z) + (scale1 * to1[2]));
        result->w = (float32)((scale0 * this->w) + (scale1 * to1[3]));
    }

    // sets quaternion from the rotation specified in the given 3x3 matrix
    BAJA_INLINE void quaternion::fromMatrix(const matrix33& matrix)
    {
        // sum the diagonal
        float32 diagSum = matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
        if (diagSum > 0.0f)
        {
            // diagonal is positive
            float32 s = sqrtf(diagSum + 1.0f);
            this->w = s / 2.0f;
            s = 0.5f / s;
            this->x = (matrix.m[1][2] - matrix.m[2][1]) * s;
            this->y = (matrix.m[2][0] - matrix.m[0][2]) * s;
            this->z = (matrix.m[0][1] - matrix.m[1][0]) * s;
        }
        else
        {
            // diagonal is either negative or zero
            uint32 nextIndex[3] = {1, 2, 0};
            uint32 i = 0;
            uint32 j;
            uint32 k;

            if (matrix.m[1][1] > matrix.m[0][0])
            {
                i = 1;
            }

            if (matrix.m[2][2] > matrix.m[i][i])
            {
                i = 2;
            }

            j = nextIndex[i];
            k = nextIndex[j];

            float32 q[4];
            float32 s = sqrtf(matrix.m[i][i] - (matrix.m[j][j] + matrix.m[k][k]) + 1.0f);
            q[i] = s * 0.5f;
            if (s != 0.0f)
            {
                s = 0.5f / s;
            }

            q[3] = (matrix.m[j][k] - matrix.m[k][j]) * s;
            q[j] = (matrix.m[i][j] - matrix.m[j][i]) * s;
            q[k] = (matrix.m[i][k] - matrix.m[k][i]) * s;

            this->x = q[0];
            this->y = q[1];
            this->z = q[2];
            this->w = q[3];
        }

        // switch signs of x,y,z 
        this->x = -this->x;
        this->y = -this->y;
        this->z = -this->z;

        // normalize
        this->normalize();
    }

    // sets quaternion from the rotation specified in the given 4x4 matrix
    BAJA_INLINE void quaternion::fromMatrix(const matrix44& matrix)
    {
        // sum the diagonal
        float32 diagSum = matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
        if (diagSum > 0.0f)
        {
            // diagonal is positive
            float32 s = sqrtf(diagSum + 1.0f);
            this->w = s / 2.0f;
            s = 0.5f / s;
            this->x = (matrix.m[1][2] - matrix.m[2][1]) * s;
            this->y = (matrix.m[2][0] - matrix.m[0][2]) * s;
            this->z = (matrix.m[0][1] - matrix.m[1][0]) * s;
        }
        else
        {
            // diagonal is either negative or zero
            uint32 nextIndex[3] = {1, 2, 0};
            uint32 i = 0;
            uint32 j;
            uint32 k;

            if (matrix.m[1][1] > matrix.m[0][0])
            {
                i = 1;
            }

            if (matrix.m[2][2] > matrix.m[i][i])
            {
                i = 2;
            }

            j = nextIndex[i];
            k = nextIndex[j];

            float32 q[4];
            float32 s = sqrtf(matrix.m[i][i] - (matrix.m[j][j] + matrix.m[k][k]) + 1.0f);
            q[i] = s * 0.5f;
            if (s != 0.0f)
            {
                s = 0.5f / s;
            }

            q[3] = (matrix.m[j][k] - matrix.m[k][j]) * s;
            q[j] = (matrix.m[i][j] - matrix.m[j][i]) * s;
            q[k] = (matrix.m[i][k] - matrix.m[k][i]) * s;

            this->x = q[0];
            this->y = q[1];
            this->z = q[2];
            this->w = q[3];
        }

        // switch signs of x,y,z 
        this->x = -this->x;
        this->y = -this->y;
        this->z = -this->z;

        // normalize
        this->normalize();
    }

    BAJA_INLINE void quaternion::toEuler(const quaternion& quat, vector3d* eulerAngles)
    {
        BAJA_VERIFY(eulerAngles);

        float32 q0 = quat.w;
        float32 q1 = quat.x;
        float32 q2 = quat.y;
        float32 q3 = quat.z;

        float32 q01 = q0 * q1;
        float32 q02 = q0 * q2;
        float32 q03 = q0 * q3;
        float32 q11 = q1 * q1;
        float32 q12 = q1 * q2;
        float32 q13 = q1 * q3;
        float32 q22 = q2 * q2;
        float32 q23 = q2 * q3;
        float32 q33 = q3 * q3;

        float32 val = (q02 - q13);
        eulerAngles->x = atan2(2 * (q01 + q23), 1 - 2 * (q11 + q22));
        eulerAngles->y = asin(2 * val);
        eulerAngles->z = atan2(2 * (q03 + q12), 1 - 2 * (q22 + q33));
    }

    // converts a quaternion into a 3x3 matrix
    BAJA_INLINE void quaternion::toMatrix(const quaternion& quat, matrix33* matrix)
    {
        // normalize the quaternion
        quaternion normalizedQuat(quat);
        normalizedQuat.normalize();

        // calculate coefficients
        float32 x2 = normalizedQuat.x + normalizedQuat.x;
        float32 y2 = normalizedQuat.y + normalizedQuat.y;
        float32 z2 = normalizedQuat.z + normalizedQuat.z;

        float32 xx = normalizedQuat.x * x2;
        float32 xy = normalizedQuat.x * y2;
        float32 xz = normalizedQuat.x * z2;

        float32 yy = normalizedQuat.y * y2;
        float32 yz = normalizedQuat.y * z2;

        float32 zz = normalizedQuat.z * z2;

        float32 wx = normalizedQuat.w * x2;
        float32 wy = normalizedQuat.w * y2;
        float32 wz = normalizedQuat.w * z2;

        // set the matrix elements
        matrix->m[0][0] = 1.0f - (yy + zz);
        matrix->m[0][1] = xy - wz;
        matrix->m[0][2] = xz + wy;

        matrix->m[1][0] = xy + wz;
        matrix->m[1][1] = 1.0f - (xx + zz);
        matrix->m[1][2] = yz - wx;

        matrix->m[2][0] = xz - wy;
        matrix->m[2][1] = yz + wx;
        matrix->m[2][2] = 1.0f - (xx + yy);

        // make sure matrix is normalized
        matrix->normalize();
    }

    // converts a quaternion into a 3x3 matrix
    BAJA_INLINE void quaternion::toMatrix(matrix33* matrix) const
    {
        quaternion::toMatrix(*this, matrix); 
    }

    // onverts a quaternion into a 4x4 matrix
    BAJA_INLINE void quaternion::toMatrix(const quaternion& quat, matrix44* matrix)
    {
        // normalize the quaternion
        quaternion normalizedQuat(quat);
        normalizedQuat.normalize();

        // calculate coefficients
        float32 x2 = normalizedQuat.x + normalizedQuat.x;
        float32 y2 = normalizedQuat.y + normalizedQuat.y;
        float32 z2 = normalizedQuat.z + normalizedQuat.z;

        float32 xx = normalizedQuat.x * x2;
        float32 xy = normalizedQuat.x * y2;
        float32 xz = normalizedQuat.x * z2;

        float32 yy = normalizedQuat.y * y2;
        float32 yz = normalizedQuat.y * z2;

        float32 zz = normalizedQuat.z * z2;

        float32 wx = normalizedQuat.w * x2;
        float32 wy = normalizedQuat.w * y2;
        float32 wz = normalizedQuat.w * z2;

        // set the matrix elements
        matrix->m[0][0] = 1.0f - (yy + zz);
        matrix->m[0][1] = xy - wz;
        matrix->m[0][2] = xz + wy;
        matrix->m[0][3] = 0.0f;

        matrix->m[1][0] = xy + wz;
        matrix->m[1][1] = 1.0f - (xx + zz);
        matrix->m[1][2] = yz - wx;
        matrix->m[1][3] = 0.0f;

        matrix->m[2][0] = xz - wy;
        matrix->m[2][1] = yz + wx;
        matrix->m[2][2] = 1.0f - (xx + yy);
        matrix->m[2][3] = 0.0f;

        matrix->m[3][0] = 0.0f;
        matrix->m[3][1] = 0.0f;
        matrix->m[3][2] = 0.0f;
        matrix->m[3][3] = 1.0f;

        // make sure matrix is normalized
        matrix->normalize();
    }

    // converts a quaternion into a 4x4 matrix
    BAJA_INLINE void quaternion::toMatrix(matrix44* matrix) const
    { 
        quaternion::toMatrix(*this, matrix); 
    }

    // matrix33 class 

    BAJA_INLINE matrix33::matrix33() 
    { 
        this->makeIdentity(); 
    }

    BAJA_INLINE matrix33::matrix33(const matrix33& src) 
    { 
        *this = src; 
    }

    BAJA_INLINE matrix33::matrix33(const matrix44& src)
    { 
        this->copyFrom(src); 
    }

    BAJA_INLINE matrix33::matrix33(const quaternion& quat) 
    { 
        quaternion::toMatrix(quat, this); 
    }

    BAJA_INLINE matrix33& matrix33::operator=(const matrix33& that) 
    { 
        this->copyFrom(that);
        return *this; // TEST: different than original
    }

    BAJA_INLINE void matrix33::getInverse(matrix33* result) const
    {
        result->m[0][0] = this->m[0][0];
        result->m[0][1] = this->m[1][0];
        result->m[0][2] = this->m[2][0];

        result->m[1][0] = this->m[0][1];
        result->m[1][1] = this->m[1][1];
        result->m[1][2] = this->m[2][1];

        result->m[2][0] = this->m[0][2];
        result->m[2][1] = this->m[1][2];
        result->m[2][2] = this->m[2][2];
    }

    // methods that apply this matrix to specific types of data

    BAJA_INLINE void matrix33::apply(const vector2d& p, vector2d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1]; 
    }

    BAJA_INLINE void matrix33::apply(const vector3d& p, vector3d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1] + p.z * this->m[0][2];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1] + p.z * this->m[1][2];
        result->z = p.x * this->m[2][0] + p.y * this->m[2][1] + p.z * this->m[2][2]; 
    }

    // applies (i.e. multiply) the matrices together
    BAJA_INLINE void matrix33::apply(const matrix33& that, matrix33* result) const
    {
        result->m[0][0] = this->m[0][0] * that.m[0][0] + this->m[0][1] * that.m[1][0] + this->m[0][2] * that.m[2][0];
        result->m[0][1] = this->m[0][0] * that.m[0][1] + this->m[0][1] * that.m[1][1] + this->m[0][2] * that.m[2][1];
        result->m[0][2] = this->m[0][0] * that.m[0][2] + this->m[0][1] * that.m[1][2] + this->m[0][2] * that.m[2][2];

        result->m[1][0] = this->m[1][0] * that.m[0][0] + this->m[1][1] * that.m[1][0] + this->m[1][2] * that.m[2][0];
        result->m[1][1] = this->m[1][0] * that.m[0][1] + this->m[1][1] * that.m[1][1] + this->m[1][2] * that.m[2][1];
        result->m[1][2] = this->m[1][0] * that.m[0][2] + this->m[1][1] * that.m[1][2] + this->m[1][2] * that.m[2][2];

        result->m[2][0] = this->m[2][0] * that.m[0][0] + this->m[2][1] * that.m[1][0] + this->m[2][2] * that.m[2][0];
        result->m[2][1] = this->m[2][0] * that.m[0][1] + this->m[2][1] * that.m[1][1] + this->m[2][2] * that.m[2][1];
        result->m[2][2] = this->m[2][0] * that.m[0][2] + this->m[2][1] * that.m[1][2] + this->m[2][2] * that.m[2][2];
    }

    // sets the 3x3 matrix to the 3x3 portion of the passed in 4x4 matrix
    BAJA_INLINE void matrix33::copyFrom(const matrix44& src)
    {
        uint32 i;

        for (i = 0; i < 3; i++)
        {
            this->m[0][i] = src.m[0][i];
            this->m[1][i] = src.m[1][i];
            this->m[2][i] = src.m[2][i];
        }
    }

    BAJA_INLINE void matrix33::copyFrom(const matrix33& src)
    {
        uint32 i;

        for (i = 0; i < 3; i++)
        {
            this->m[0][i] = src.m[0][i];
            this->m[1][i] = src.m[1][i];
            this->m[2][i] = src.m[2][i];
        }
    }

    // set rotation about a specified axis angle
    BAJA_INLINE void matrix33::setRotationAxisAngle(const vector3d* axis, float32 angle)
    {
        float32 xx = axis->x * axis->x;
        float32 xy = axis->x * axis->y;
        float32 xz = axis->x * axis->z;

        float32 yy = axis->y * axis->y;
        float32 yz = axis->y * axis->z;

        float32 zz = axis->z * axis->z;

        float32 sn = ::sinf(angle);
        float32 cs = ::cosf(angle);

        float32 oneMinusCs = 1 - cs;

        this->m[0][0] = cs + xx * oneMinusCs;
        this->m[1][0] = oneMinusCs * xy + axis->z * sn;
        this->m[2][0] = oneMinusCs * xz - axis->y * sn;

        this->m[0][1] = oneMinusCs * xy - axis->z * sn;
        this->m[1][1] = cs + yy * oneMinusCs;
        this->m[2][1] = oneMinusCs * yz + axis->x * sn;

        this->m[0][2] = oneMinusCs * xz + axis->y * sn;
        this->m[1][2] = oneMinusCs * yz - axis->x * sn;
        this->m[2][2] = cs + zz * oneMinusCs;
    }

    // sets the rotation for this matrix
    BAJA_INLINE void matrix33::setRotation(float32 yaw, float32 pitch, float32 roll)
    {
        float32 Ya = cos(yaw);
        float32 Yc = sin(yaw);
        float32 Yb = -Yc;

        float32 Pa = cos(pitch);
        float32 Pc = sin(pitch);
        float32 Pb = -Pc;

        float32 Ra = cos(roll);
        float32 Rc = sin(roll);
        float32 Rb = -Rc;

        float32 YbPb = Yb * Pb;
        float32 YaPb = Ya * Pb;

        this->m[0][0] = Ya * Ra + YbPb * Rc;
        this->m[0][1] = Yb * Pa;
        this->m[0][2] = Ya * Rb + YbPb * Ra;

        this->m[1][0] = Yc * Ra + YaPb * Rc;
        this->m[1][1] = Ya * Pa;
        this->m[1][2] = Yc * Rb + YaPb * Ra;

        this->m[2][0] = Pa * Rc;
        this->m[2][1] = Pc;
        this->m[2][2] = Pa * Ra;
    }

    // makes matrix into an identity matrix
    BAJA_INLINE void matrix33::makeIdentity()
    {
        this->m[0][0] = 1.0f;
        this->m[0][1] = 0.0f;
        this->m[0][2] = 0.0f;

        this->m[1][0] = 0.0f;
        this->m[1][1] = 1.0f;
        this->m[1][2] = 0.0f;

        this->m[2][0] = 0.0f;
        this->m[2][1] = 0.0f;
        this->m[2][2] = 1.0f;
    }

    // inverts this matrix
    BAJA_INLINE void matrix33::invert()
    {
        matrix33 t;

        this->getInverse(&t);

        *this = t;
    }

    // normalizes this matrix. this helps negate affects of floating point rounding errors
    // that, over time, can cause a matrix to lose its orthogonal properties
    BAJA_INLINE void matrix33::normalize()
    {
        float32 magnitudeSq;
        float32 magnitude;
        float32 magnitudeInv;
        uint32 i;

        for (i = 0; i < 3; i++)
        {
            magnitudeSq = this->m[0][i] * this->m[0][i] + this->m[1][i] * this->m[1][i] + this->m[2][i] * this->m[2][i];
            magnitude = sqrtf(magnitudeSq);

            magnitudeInv = (fabs(magnitude) > c::floatEpsilon) ? 1.0f / magnitude : 0.0f;

            this->m[0][i] *= magnitudeInv;
            this->m[1][i] *= magnitudeInv;
            this->m[2][i] *= magnitudeInv;
        }
    }

    // matrix44

    BAJA_INLINE matrix44::matrix44()
    {
        this->makeIdentity();
    }

    BAJA_INLINE matrix44::matrix44(const matrix44& src) 
    { 
        *this = src; 
    }

    BAJA_INLINE matrix44::matrix44(const matrix33& src)
    { 
        this->copyFrom(src); 
    }

    BAJA_INLINE matrix44::matrix44(const quaternion& quat)
    {  
        quaternion::toMatrix(quat, this); 
    }

    BAJA_INLINE matrix44::matrix44(const float32 scale)
    { 
        this->setScale(scale, scale, scale); 

        this->m[0][3] = 0.0f;
        this->m[1][3] = 0.0f;
        this->m[2][3] = 0.0f;

        this->m[3][0] = 0.0f;
        this->m[3][1] = 0.0f;
        this->m[3][2] = 0.0f;
        this->m[3][3] = 1.0f; 
    }

    BAJA_INLINE matrix44::matrix44(float32 xScale, float32 yScale, float32 zScale)
    { 
        this->setScale(xScale, yScale, zScale); 

        this->m[0][3] = 0.0f;
        this->m[1][3] = 0.0f;
        this->m[2][3] = 0.0f;

        this->m[3][0] = 0.0f;
        this->m[3][1] = 0.0f;
        this->m[3][2] = 0.0f;
        this->m[3][3] = 1.0f; 
    }

    BAJA_INLINE matrix44& matrix44::operator=(const matrix44& that)
    {
        this->copyFrom(that); // TEST: different than original
        return *this;
    }

    BAJA_INLINE void matrix44::getTranslation(point3d* result) const
    { 
        result->x = this->m[0][3];
        result->y = this->m[1][3];
        result->z = this->m[2][3]; 
    }

    BAJA_INLINE void matrix44::decompose(
        vector3d* scale,
        vector3d* eulerXYZ,
        point3d* translation
        ) const
    {
        if (scale == nullptr|| eulerXYZ == nullptr || translation == nullptr)
        {
            return;
        }

        matrix44 normalized = *this;
        normalized.normalize();
        normalized.getEulerXYZ(eulerXYZ);

        this->getScale(scale);
        this->getTranslation(translation);
    }

    BAJA_INLINE bool matrix44::decompose(
        vector3d* outScale,
        float32* outShearXY,
        float32* outShearXZ,
        float32* outShearYZ,
        point4d* outPersp,
        vector3d* outEulerXYZ,
        point3d* outTranslation
        ) const
    {
        BAJA_VERIFY(outScale);
        BAJA_VERIFY(outShearXY);
        BAJA_VERIFY(outShearXZ);
        BAJA_VERIFY(outShearYZ);
        BAJA_VERIFY(outPersp);
        BAJA_VERIFY(outEulerXYZ);
        BAJA_VERIFY(outTranslation);

        matrix44 localCopy = *this;

        // divide the rows by the 3,3 element of matrix
        // if this element is == 0 return failure
        float32 m33 = this->m[3][3];
        if (fabsf(m33) <= c::floatEpsilon)
        {
            return false;
        }

        float32 m33Inv = 1.0f / m33;
        for (uint32 i = 0; i < 4; i++)
        {
            for (uint32 j = 0; j < 4; j++)
            {
                localCopy.m[i][j] *= m33Inv;
            }
        }

        matrix44 perspectiveMatrix = localCopy;

        // clear the perspective row
        perspectiveMatrix.m[3][0] = 0;
        perspectiveMatrix.m[3][1] = 0;
        perspectiveMatrix.m[3][2] = 0;
        perspectiveMatrix.m[3][3] = 1;

        // check if the matrix is singular
        float64 det = perspectiveMatrix.getDeterminant();
        if (abs(det) < DBL_EPSILON)
        {
            return false;
        }

        // process the persp part of the transform
        float32 perspX = localCopy.m[3][0];
        float32 perspY = localCopy.m[3][1];
        float32 perspZ = localCopy.m[3][2];
        if (fabsf(perspX) > c::floatEpsilon || fabsf(perspY) > c::floatEpsilon || fabsf(perspZ) > c::floatEpsilon)
        {
            // right hand side
            point4d rhs;
            rhs.x = localCopy.m[3][0];
            rhs.y = localCopy.m[3][1];
            rhs.z = localCopy.m[3][2];
            rhs.w = localCopy.m[3][3];

            matrix44 invPerspMatrix;
            perspectiveMatrix.getInverse(&invPerspMatrix, &det);

            matrix44 invPerspMatrixTransposed;
            invPerspMatrix.getTranspose(&invPerspMatrixTransposed);

            invPerspMatrixTransposed.apply(rhs, outPersp);

            // clear the persp data
            localCopy.m[3][0] = 0;
            localCopy.m[3][1] = 0;
            localCopy.m[3][2] = 0;
            localCopy.m[3][3] = 1;
        }
        else
        {
            outPersp->x = 0;
            outPersp->y = 0;
            outPersp->z = 0;
            outPersp->w = 0;
        }

        localCopy.getTranslation(outTranslation);
        localCopy.setTranslation(point3d(0, 0, 0));

        // process row 0 scale
        vector3d row0;
        vector3d row1;
        vector3d row2;
        localCopy.getRightVector(&row0);
        localCopy.getUpVector(&row1);
        localCopy.getForwardVector(&row2);

        outScale->x = row0.length();
        row0.normalize();

        *outShearXY = row0.dot(row1);
        linearCombine(row1, 1, row0, -(*outShearXY), &row1);

        outScale->y = row1.length();
        row1.normalize();
        *outShearXY /= outScale->y;

        *outShearXZ = row0.dot(row2);
        linearCombine(row2, 1, row0, -(*outShearXZ), &row2);

        *outShearYZ = row1.dot(row2);
        linearCombine(row2, 1, row1, -(*outShearYZ), &row2);

        outScale->z = row2.length();
        row2.normalize();

        *outShearXZ /= outScale->z;
        *outShearYZ /= outScale->z;

        // check for coord system flip
        vector3d theCross;
        row1.cross(row2, &theCross);

        if (theCross.dot(row0) < 0)
        {
            // flip, invert scale
            outScale->x = -outScale->x;
            outScale->y = -outScale->y;
            outScale->z = -outScale->z;

            row0.scale(-1);
            row1.scale(-1);
            row2.scale(-1);
        }

        matrix44 rotationMatrix;
        rotationMatrix.set(row0, row1, row2);

        rotationMatrix.getEulerXYZ(outEulerXYZ);

        return true;
    }

    BAJA_INLINE void matrix44::getEulerXYZ(vector3d* result) const
    {
        BAJA_VERIFY(result);

        float32 m20 = this->m[2][0];
        if (fabs(m20) < 1)
        {
            // note: multiple solutions can be computed...
            // second solution below is commented out

            float32 ry1 = -asinf(m20);
            // float32 ry2 = kGxPi - ry1;

            float32 c1 = cosf(ry1);
            // float32 c2 = cosf(ry2);

            float32 rx1 = atan2f(m[2][1]/c1, m[2][2]/c1);
            // float32 rx2 = atan2f(m[2][1]/c2, m[2][2]/c2);

            float32 rz1 = atan2f(m[1][0]/c1, m[0][0]/c1);
            // float32 rz2 = atan2f(m[1][0]/c2, m[0][0]/c2);

            result->x = rx1;
            result->y = ry1;
            result->z = rz1;
        }
        else
        {
            float32 rx = 0;
            float32 ry = 0;
            float32 rz = 0;
            if (m20 < 0)
            {
                ry = c::pi * 0.5f;
                rx = rz + atan2f(m[0][1], m[0][2]);
            }
            else
            {
                ry = -c::pi * 0.5f;
                rx = -rz + atan2f(-m[0][1], -m[0][2]);
            }
            result->x = rx;
            result->y = ry;
            result->z = rz;
        }
    }

    BAJA_INLINE void matrix44::getScale(vector3d* result) const
    {
        result->x = sqrtf(this->m[0][0] * this->m[0][0] + this->m[1][0] * this->m[1][0] + this->m[2][0] * this->m[2][0]);
        result->y = sqrtf(this->m[0][1] * this->m[0][1] + this->m[1][1] * this->m[1][1] + this->m[2][1] * this->m[2][1]);
        result->z = sqrtf(this->m[0][2] * this->m[0][2] + this->m[1][2] * this->m[1][2] + this->m[2][2] * this->m[2][2]);
    }

    BAJA_INLINE float64 matrix44::getDeterminant() const
    {
        float64 returnVal = 0;

        returnVal = (float64)
            m[0][3]*m[1][2]*m[2][1]*m[3][0] - m[0][2]*m[1][3]*m[2][1]*m[3][0] -
            m[0][3]*m[1][1]*m[2][2]*m[3][0] + m[0][1]*m[1][3]*m[2][2]*m[3][0] +
            m[0][2]*m[1][1]*m[2][3]*m[3][0] - m[0][1]*m[1][2]*m[2][3]*m[3][0] -
            m[0][3]*m[1][2]*m[2][0]*m[3][1] + m[0][2]*m[1][3]*m[2][0]*m[3][1] +
            m[0][3]*m[1][0]*m[2][2]*m[3][1] - m[0][0]*m[1][3]*m[2][2]*m[3][1] -
            m[0][2]*m[1][0]*m[2][3]*m[3][1] + m[0][0]*m[1][2]*m[2][3]*m[3][1] +
            m[0][3]*m[1][1]*m[2][0]*m[3][2] - m[0][1]*m[1][3]*m[2][0]*m[3][2] -
            m[0][3]*m[1][0]*m[2][1]*m[3][2] + m[0][0]*m[1][3]*m[2][1]*m[3][2] +
            m[0][1]*m[1][0]*m[2][3]*m[3][2] - m[0][0]*m[1][1]*m[2][3]*m[3][2] -
            m[0][2]*m[1][1]*m[2][0]*m[3][3] + m[0][1]*m[1][2]*m[2][0]*m[3][3] +
            m[0][2]*m[1][0]*m[2][1]*m[3][3] - m[0][0]*m[1][2]*m[2][1]*m[3][3] -
            m[0][1]*m[1][0]*m[2][2]*m[3][3] + m[0][0]*m[1][1]*m[2][2]*m[3][3];

        /*det = m03 * m12 * m21 * m30 - m02 * m13 * m21 * m30-
        m03 * m11 * m22 * m30+m01 * m13 * m22 * m30+
        m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-
        m03 * m12 * m20 * m31+m02 * m13 * m20 * m31+
        m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-
        m02 * m10 * m23 * m31+m00 * m12 * m23 * m31+
        m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-
        m03 * m10 * m21 * m32+m00 * m13 * m21 * m32+
        m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-
        m02 * m11 * m20 * m33+m01 * m12 * m20 * m33+
        m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-
        m01 * m10 * m22 * m33+m00 * m11 * m22 * m33
        */
        return returnVal;
    }

    BAJA_INLINE void matrix44::getInverse(matrix44* result, float64* outDeterminant) const
    {
        float64 det = this->getDeterminant();
        if (outDeterminant)
        {
            *outDeterminant = det;
        }

        if (abs(det) < DBL_EPSILON)
        {
            return;
        }

        // set our result to be the RHS of the augmented matrix
        ((matrix44*)result)->makeIdentity();

        float64 tmpMatrix[4][4];

        // make identity
        float64 result64[4][4] =
        {
            {1, 0, 0, 0},
            {0, 1, 0, 0},
            {0, 0, 1, 0},
            {0, 0, 0, 1},
        };

        float64* mat64 = (float64*)tmpMatrix;
        float32* mat32 = (float32*)this->m;
        for (uint32 i = 0; i < 16; i++)
        {
            mat64[i] = mat32[i];
        }

        // for each row...
        for (uint32 cursor = 0; cursor < 4; cursor++)
        {
            // ensure value at our cursor location is non zero
            if (abs(tmpMatrix[cursor][cursor]) < DBL_EPSILON)
            {
                tmpMatrix[cursor][cursor] = 0;

                // swap
                for (uint32 i = cursor + 1; i < 4; i++)
                {
                    if (abs(tmpMatrix[i][cursor]) >= DBL_EPSILON)
                    {
                        float64 tmp[4]; 
                        std::memcpy(tmp, tmpMatrix[i], sizeof(tmp));
                        std::memcpy(tmpMatrix[i], tmpMatrix[cursor], sizeof(tmpMatrix[i]));
                        std::memcpy(tmpMatrix[cursor], tmp, sizeof(tmpMatrix[cursor]));

                        // swap the rows of the augmented matrix
                        std::memcpy(tmp, result64[i], sizeof(tmp));
                        std::memcpy(result64[i], result64[cursor], sizeof(result64[i]));
                        std::memcpy(result64[cursor], tmp, sizeof(result64[cursor]));

                        break;
                    }
                }
            }
            float64 newCursorValue = tmpMatrix[cursor][cursor];
            BAJA_VERIFY(newCursorValue != 0);

            // divide row such that value at cursor location == 1
            if (newCursorValue != 1)
            {
                float64 invCursor = 1/newCursorValue;
                for (uint32 i = 0; i < 4; i++)
                {
                    tmpMatrix[cursor][i] *= invCursor;
                    result64[cursor][i] *= invCursor;
                }
            }

            // after the above loop this will be very close to 1, set to 1 here to prevent precision errors
            tmpMatrix[cursor][cursor] = 1;

            // now that cursor value is 1, subtract from rows below to zero out leading column values
            for (uint32 i = 0; i < 4; i++)
            {
                if (i != cursor)
                {
                    float64 scale = tmpMatrix[i][cursor];
                    for (uint32 j = 0; j < 4; j++)
                    {
                        tmpMatrix[i][j] -= scale * tmpMatrix[cursor][j];
                        result64[i][j] -= scale * result64[cursor][j];
                    }
                }
            }
        }

        // copy back into the low prec
        mat64 = (float64*)result64;
        mat32 = (float32*)result->m;
        for (uint32 i = 0; i < 16; i++)
        {
            mat32[i] = (float32)mat64[i];
        }

#ifdef BAJA_DEBUG
        for (uint32 i = 0; i < 4; i++)
        {
            for (uint32 j = 0; j < 4; j++)
            {
                BAJA_VERIFY(abs(tmpMatrix[i][j] - (i == j ? 1 : 0)) < DBL_EPSILON);
            }
        }
#endif
    }

    BAJA_INLINE void matrix44::getInverse(matrix44* result) const
    {
        // determine scale factor in X, Y and Z
        float32 xScale = this->m[0][0] * this->m[0][0] + this->m[1][0] * this->m[1][0] + this->m[2][0] * this->m[2][0];
        if (xScale != 0.0f)
        {
            xScale = 1.0f / xScale;
        }

        float32 yScale = this->m[0][1] * this->m[0][1] + this->m[1][1] * this->m[1][1] + this->m[2][1] * this->m[2][1];
        if (yScale != 0.0f)
        {
            yScale = 1.0f / yScale;
        }

        float32 zScale = this->m[0][2] * this->m[0][2] + this->m[1][2] * this->m[1][2] + this->m[2][2] * this->m[2][2];
        if (zScale != 0.0f)
        {
            zScale = 1.0f / zScale;
        }

        point3d trans;
        trans.x = -this->m[0][3];
        trans.y = -this->m[1][3];
        trans.z = -this->m[2][3];

        // transpose the matrix, factoring scale into the equation
        result->m[0][0] = this->m[0][0] * xScale;
        result->m[0][1] = this->m[1][0] * xScale;
        result->m[0][2] = this->m[2][0] * xScale;
        result->m[0][3] = trans.x * result->m[0][0] + trans.y * result->m[0][1] + trans.z * result->m[0][2];

        result->m[1][0] = this->m[0][1] * yScale;
        result->m[1][1] = this->m[1][1] * yScale;
        result->m[1][2] = this->m[2][1] * yScale;
        result->m[1][3] = trans.x * result->m[1][0] + trans.y * result->m[1][1] + trans.z * result->m[1][2];

        result->m[2][0] = this->m[0][2] * zScale;
        result->m[2][1] = this->m[1][2] * zScale;
        result->m[2][2] = this->m[2][2] * zScale;
        result->m[2][3] = trans.x * result->m[2][0] + trans.y * result->m[2][1] + trans.z * result->m[2][2];

        result->m[3][0] = 0.0f;
        result->m[3][1] = 0.0f;
        result->m[3][2] = 0.0f;
        result->m[3][3] = 1.0f;
    }

    BAJA_INLINE void matrix44::getTranspose(matrix44* result) const
    {
        result->m[0][0] = this->m[0][0];
        result->m[0][1] = this->m[1][0];
        result->m[0][2] = this->m[2][0];
        result->m[0][3] = this->m[3][0];

        result->m[1][0] = this->m[0][1];
        result->m[1][1] = this->m[1][1];
        result->m[1][2] = this->m[2][1];
        result->m[1][3] = this->m[3][1];

        result->m[2][0] = this->m[0][2];
        result->m[2][1] = this->m[1][2];
        result->m[2][2] = this->m[2][2];
        result->m[2][3] = this->m[3][2];

        result->m[3][0] = this->m[0][3];
        result->m[3][1] = this->m[1][3];
        result->m[3][2] = this->m[2][3];
        result->m[3][3] = this->m[3][3];
    }

    BAJA_INLINE void matrix44::set(const vector3d& right, const vector3d& up, const vector3d& forward)
    {
        this->m[0][0] = right.x;
        this->m[0][1] = up.x;
        this->m[0][2] = forward.x;

        this->m[1][0] = right.y;
        this->m[1][1] = up.y;
        this->m[1][2] = forward.y;

        this->m[2][0] = right.z;
        this->m[2][1] = up.z;
        this->m[2][2] = forward.z;
    }

    BAJA_INLINE void matrix44::setRightVector(const vector3d& right)
    {
        this->m[0][0] = right.x;
        this->m[1][0] = right.y;
        this->m[2][0] = right.z;
    }

    BAJA_INLINE void matrix44::setUpVector(const vector3d& up)
    {
        this->m[0][1] = up.x;
        this->m[1][1] = up.y;
        this->m[2][1] = up.z;
    }

    BAJA_INLINE void matrix44::setForwardVector(const vector3d& forward)
    {
        this->m[0][2] = forward.x;
        this->m[1][2] = forward.y;
        this->m[2][2] = forward.z;
    }

    BAJA_INLINE void matrix44::getRightVector(vector3d* outRight) const
    {
        if (outRight)
        {
            outRight->x = this->m[0][0];
            outRight->y = this->m[1][0];
            outRight->z = this->m[2][0];
        }
    }

    BAJA_INLINE void matrix44::getUpVector(vector3d* outUp) const
    {
        if (outUp)
        {
            outUp->x = this->m[0][1];
            outUp->y = this->m[1][1];
            outUp->z = this->m[2][1];
        }
    }

    BAJA_INLINE void matrix44::getForwardVector(vector3d* outForward) const
    {
        if (outForward)
        {
            outForward->x = this->m[0][2];
            outForward->y = this->m[1][2];
            outForward->z = this->m[2][2];
        }
    }

    BAJA_INLINE float32 matrix44::getHandednessSign() const
    {
        vector3d x;
        vector3d y;
        vector3d z;

        this->getRightVector(&x);
        this->getUpVector(&y);
        this->getForwardVector(&z);

        vector3d cross;
        x.cross(y, &cross);

        float32 thedot = cross.dot(z);
        if (thedot < 0)
        {
            return -1;
        }
        return 1;
    }

    BAJA_INLINE void matrix44::apply(const point2d& p, point2d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1] + this->m[0][3];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1] + this->m[1][3]; 
    }

    BAJA_INLINE void matrix44::apply(const vector2d& p, vector2d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1]; 
    }

    BAJA_INLINE void matrix44::apply(const point3d& p, point3d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1] + p.z * this->m[0][2] + this->m[0][3];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1] + p.z * this->m[1][2] + this->m[1][3];
        result->z = p.x * this->m[2][0] + p.y * this->m[2][1] + p.z * this->m[2][2] + this->m[2][3]; 
    }

    BAJA_INLINE void matrix44::apply(const vector3d& p, vector3d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1] + p.z * this->m[0][2];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1] + p.z * this->m[1][2];
        result->z = p.x * this->m[2][0] + p.y * this->m[2][1] + p.z * this->m[2][2]; 
    }

    BAJA_INLINE void matrix44::apply(const point4d& p, point4d* result) const
    { 
        result->x = p.x * this->m[0][0] + p.y * this->m[0][1] + p.z * this->m[0][2] + p.w * this->m[0][3];
        result->y = p.x * this->m[1][0] + p.y * this->m[1][1] + p.z * this->m[1][2] + p.w * this->m[1][3];
        result->z = p.x * this->m[2][0] + p.y * this->m[2][1] + p.z * this->m[2][2] + p.w * this->m[2][3]; 
        result->w = p.x * this->m[3][0] + p.y * this->m[3][1] + p.z * this->m[3][2] + p.w * this->m[3][3]; 
    }

    BAJA_INLINE void matrix44::apply(const line3d& p, line3d* result) const
    { 
        this->apply(p.point, &result->point);
        this->apply(p.direction, &result->direction); 
    }

    BAJA_INLINE void matrix44::apply(const matrix44& that, matrix44* result) const
    {
        result->m[0][0] = this->m[0][0] * that.m[0][0] + this->m[0][1] * that.m[1][0] + this->m[0][2] * that.m[2][0];
        result->m[0][1] = this->m[0][0] * that.m[0][1] + this->m[0][1] * that.m[1][1] + this->m[0][2] * that.m[2][1];
        result->m[0][2] = this->m[0][0] * that.m[0][2] + this->m[0][1] * that.m[1][2] + this->m[0][2] * that.m[2][2];
        result->m[0][3] = this->m[0][0] * that.m[0][3] + this->m[0][1] * that.m[1][3] + this->m[0][2] * that.m[2][3] + this->m[0][3];

        result->m[1][0] = this->m[1][0] * that.m[0][0] + this->m[1][1] * that.m[1][0] + this->m[1][2] * that.m[2][0];
        result->m[1][1] = this->m[1][0] * that.m[0][1] + this->m[1][1] * that.m[1][1] + this->m[1][2] * that.m[2][1];
        result->m[1][2] = this->m[1][0] * that.m[0][2] + this->m[1][1] * that.m[1][2] + this->m[1][2] * that.m[2][2];
        result->m[1][3] = this->m[1][0] * that.m[0][3] + this->m[1][1] * that.m[1][3] + this->m[1][2] * that.m[2][3] + this->m[1][3];

        result->m[2][0] = this->m[2][0] * that.m[0][0] + this->m[2][1] * that.m[1][0] + this->m[2][2] * that.m[2][0];
        result->m[2][1] = this->m[2][0] * that.m[0][1] + this->m[2][1] * that.m[1][1] + this->m[2][2] * that.m[2][1];
        result->m[2][2] = this->m[2][0] * that.m[0][2] + this->m[2][1] * that.m[1][2] + this->m[2][2] * that.m[2][2];
        result->m[2][3] = this->m[2][0] * that.m[0][3] + this->m[2][1] * that.m[1][3] + this->m[2][2] * that.m[2][3] + this->m[2][3];

        result->m[3][0] = 0.0f;
        result->m[3][1] = 0.0f;
        result->m[3][2] = 0.0f;
        result->m[3][3] = 1.0f;
    }

    BAJA_INLINE void matrix44::applyFully(const matrix44& that, matrix44* result) const
    {
        result->m[0][0] = this->m[0][0] * that.m[0][0] + this->m[0][1] * that.m[1][0] + this->m[0][2] * that.m[2][0] + this->m[0][3] * that.m[3][0];
        result->m[0][1] = this->m[0][0] * that.m[0][1] + this->m[0][1] * that.m[1][1] + this->m[0][2] * that.m[2][1] + this->m[0][3] * that.m[3][1];
        result->m[0][2] = this->m[0][0] * that.m[0][2] + this->m[0][1] * that.m[1][2] + this->m[0][2] * that.m[2][2] + this->m[0][3] * that.m[3][2];
        result->m[0][3] = this->m[0][0] * that.m[0][3] + this->m[0][1] * that.m[1][3] + this->m[0][2] * that.m[2][3] + this->m[0][3] * that.m[3][3];

        result->m[1][0] = this->m[1][0] * that.m[0][0] + this->m[1][1] * that.m[1][0] + this->m[1][2] * that.m[2][0] + this->m[1][3] * that.m[3][0];
        result->m[1][1] = this->m[1][0] * that.m[0][1] + this->m[1][1] * that.m[1][1] + this->m[1][2] * that.m[2][1] + this->m[1][3] * that.m[3][1];
        result->m[1][2] = this->m[1][0] * that.m[0][2] + this->m[1][1] * that.m[1][2] + this->m[1][2] * that.m[2][2] + this->m[1][3] * that.m[3][2];
        result->m[1][3] = this->m[1][0] * that.m[0][3] + this->m[1][1] * that.m[1][3] + this->m[1][2] * that.m[2][3] + this->m[1][3] * that.m[3][3];

        result->m[2][0] = this->m[2][0] * that.m[0][0] + this->m[2][1] * that.m[1][0] + this->m[2][2] * that.m[2][0] + this->m[2][3] * that.m[3][0];
        result->m[2][1] = this->m[2][0] * that.m[0][1] + this->m[2][1] * that.m[1][1] + this->m[2][2] * that.m[2][1] + this->m[2][3] * that.m[3][1];
        result->m[2][2] = this->m[2][0] * that.m[0][2] + this->m[2][1] * that.m[1][2] + this->m[2][2] * that.m[2][2] + this->m[2][3] * that.m[3][2];
        result->m[2][3] = this->m[2][0] * that.m[0][3] + this->m[2][1] * that.m[1][3] + this->m[2][2] * that.m[2][3] + this->m[2][3] * that.m[3][3];

        result->m[3][0] = this->m[3][0] * that.m[0][0] + this->m[3][1] * that.m[1][0] + this->m[3][2] * that.m[2][0] + this->m[3][3] * that.m[3][0];
        result->m[3][1] = this->m[3][0] * that.m[0][1] + this->m[3][1] * that.m[1][1] + this->m[3][2] * that.m[2][1] + this->m[3][3] * that.m[3][1];
        result->m[3][2] = this->m[3][0] * that.m[0][2] + this->m[3][1] * that.m[1][2] + this->m[3][2] * that.m[2][2] + this->m[3][3] * that.m[3][2];
        result->m[3][3] = this->m[3][0] * that.m[0][3] + this->m[3][1] * that.m[1][3] + this->m[3][2] * that.m[2][3] + this->m[3][3] * that.m[3][3];
    }

    BAJA_INLINE void matrix44::copyFrom(const matrix44& src)
    {
        this->m[0][0] = src.m[0][0];
        this->m[0][1] = src.m[0][1];
        this->m[0][2] = src.m[0][2];
        this->m[0][3] = src.m[0][3];

        this->m[1][0] = src.m[1][0];
        this->m[1][1] = src.m[1][1];
        this->m[1][2] = src.m[1][2];
        this->m[1][3] = src.m[1][3];

        this->m[2][0] = src.m[2][0];
        this->m[2][1] = src.m[2][1];
        this->m[2][2] = src.m[2][2];
        this->m[2][3] = src.m[2][3];

        this->m[3][0] = src.m[3][0];
        this->m[3][1] = src.m[3][1];
        this->m[3][2] = src.m[3][2];
        this->m[3][3] = src.m[3][3];
    }

    BAJA_INLINE void matrix44::copyFrom(const matrix33& src)
    {
        this->m[0][0] = src.m[0][0];
        this->m[0][1] = src.m[0][1];
        this->m[0][2] = src.m[0][2];
        this->m[0][3] = 0.0f;

        this->m[1][0] = src.m[1][0];
        this->m[1][1] = src.m[1][1];
        this->m[1][2] = src.m[1][2];
        this->m[1][3] = 0.0f;

        this->m[2][0] = src.m[2][0];
        this->m[2][1] = src.m[2][1];
        this->m[2][2] = src.m[2][2];
        this->m[2][3] = 0.0f;

        this->m[3][0] = 0.0f;
        this->m[3][1] = 0.0f;
        this->m[3][2] = 0.0f;
        this->m[3][3] = 1.0f;
    }

    BAJA_INLINE void matrix44::setTranslation(const point3d& translation)
    { 
        this->m[0][3] = translation.x;
        this->m[1][3] = translation.y;
        this->m[2][3] = translation.z; 
    }

    BAJA_INLINE void matrix44::setRotationZXY(float32 z, float32 x, float32 y)
    {
    }

    BAJA_INLINE void matrix44::setRotationAxisAngle(const vector3d* axis, float32 angle)
    {
        float32 xx = axis->x * axis->x;
        float32 xy = axis->x * axis->y;
        float32 xz = axis->x * axis->z;

        float32 yy = axis->y * axis->y;
        float32 yz = axis->y * axis->z;

        float32 zz = axis->z * axis->z;

        float32 sn = ::sinf(angle);
        float32 cs = ::cosf(angle);

        float32 oneMinusCs = 1 - cs;

        this->m[0][0] = cs + xx * oneMinusCs;
        this->m[1][0] = oneMinusCs * xy + axis->z * sn;
        this->m[2][0] = oneMinusCs * xz - axis->y * sn;

        this->m[0][1] = oneMinusCs * xy - axis->z * sn;
        this->m[1][1] = cs + yy * oneMinusCs;
        this->m[2][1] = oneMinusCs * yz + axis->x * sn;

        this->m[0][2] = oneMinusCs * xz + axis->y * sn;
        this->m[1][2] = oneMinusCs * yz - axis->x * sn;
        this->m[2][2] = cs + zz * oneMinusCs;
    }

    BAJA_INLINE void matrix44::setRotationX(float32 angle)
    {
        float32 sn = sinf(angle);
        float32 cs = cosf(angle);

        this->m[0][0] = 1;
        this->m[0][1] = 0;
        this->m[0][2] = 0;

        this->m[1][0] = 0;
        this->m[1][1] = cs;
        this->m[1][2] = -sn;

        this->m[2][0] = 0;
        this->m[2][1] = sn;
        this->m[2][2] = cs;
    }

    BAJA_INLINE void matrix44::setRotationY(float32 angle)
    {
        float32 sn = sinf(angle);
        float32 cs = cosf(angle);

        this->m[0][0] = cs;
        this->m[0][1] = 0;
        this->m[0][2] = sn;

        this->m[1][0] = 0;
        this->m[1][1] = 1;
        this->m[1][2] = 0;

        this->m[2][0] = -sn;
        this->m[2][1] = 0;
        this->m[2][2] = cs;
    }

    BAJA_INLINE void matrix44::setRotationZ(float32 angle)
    {
        float32 sn = sinf(angle);
        float32 cs = cosf(angle);

        this->m[0][0] = cs;
        this->m[0][1] = -sn;
        this->m[0][2] = 0;

        this->m[1][0] = sn;
        this->m[1][1] = cs;
        this->m[1][2] = 0;

        this->m[2][0] = 0;
        this->m[2][1] = 0;
        this->m[2][2] = 1;
    }

    // The order the rotations are applied is: X first, Y second, Z last
    BAJA_INLINE void matrix44::setRotationXYZ(
        float32 rotX,
        float32 rotY,
        float32 rotZ
        )
    {
        float32 cx = cosf(rotX);
        float32 cy = cosf(rotY);
        float32 cz = cosf(rotZ);

        float32 sx = sinf(rotX);
        float32 sy = sinf(rotY);
        float32 sz = sinf(rotZ);

        float32 cycz = cz * cy;
        float32 cysz = sz * cy;

        float32 sxsy = sy * sx;

        float32 sxsycz = sxsy * cz;
        float32 sxsysz = sxsy * sz;
        float32 sxcy = sx * cy;

        float32 cxsz = cx * sz;
        float32 cxcz = cx * cz;

        float32 cxsycz = cx * sy * cz;
        float32 cxsysz = cx * sy * sz;

        float32 sxsz = sx * sz;
        float32 sxcz = sx * cz;
        float32 cxcy = cx * cy;

        this->m[0][0] = cycz;
        this->m[1][0] = cysz;
        this->m[2][0] = -sy;

        this->m[0][1] = sxsycz - cxsz;
        this->m[1][1] = sxsysz + cxcz;
        this->m[2][1] = sxcy;

        this->m[0][2] = cxsycz + sxsz;
        this->m[1][2] = cxsysz - sxcz;
        this->m[2][2] = cxcy;
    }

    BAJA_INLINE void matrix44::setScale(float32 scale)
    { 
        this->setScale(scale, scale, scale); 
    }

    BAJA_INLINE void matrix44::setScale(float32 xScale, float32 yScale, float32 zScale)
    {
        this->m[0][0] = xScale;
        this->m[0][1] = 0.0f;
        this->m[0][2] = 0.0f;

        this->m[1][0] = 0.0f;
        this->m[1][1] = yScale;
        this->m[1][2] = 0.0f;

        this->m[2][0] = 0.0f;
        this->m[2][1] = 0.0f;
        this->m[2][2] = zScale;
    }

    BAJA_INLINE void matrix44::makeIdentity()
    {
        this->m[0][0] = 1.0f;
        this->m[0][1] = 0.0f;
        this->m[0][2] = 0.0f;
        this->m[0][3] = 0.0f;

        this->m[1][0] = 0.0f;
        this->m[1][1] = 1.0f;
        this->m[1][2] = 0.0f;
        this->m[1][3] = 0.0f;

        this->m[2][0] = 0.0f;
        this->m[2][1] = 0.0f;
        this->m[2][2] = 1.0f;
        this->m[2][3] = 0.0f;

        this->m[3][0] = 0.0f;
        this->m[3][1] = 0.0f;
        this->m[3][2] = 0.0f;
        this->m[3][3] = 1.0f;
    }

    BAJA_INLINE void matrix44::makeCamera(const point3d& lookFrom, const point3d& lookAt, const matrix44& coordSystem)
    {
        matrix44 coords = coordSystem;

        matrix44 invCoords;
        coords.getTranspose(&invCoords);

        point3d eye;
        invCoords.apply(lookFrom, &eye);

        point3d lookAtInFinalCoords;
        invCoords.apply(lookAt, &lookAtInFinalCoords);

        vector3d up(0, 1, 0);
        vector3d fwd(0, 0, 1);

        vector3d viewDirection;
        eye.vectorTo(lookAtInFinalCoords, &viewDirection);
        viewDirection.normalize();

        vector3d lookAtRightVector;
        vector3d lookAtUpVector;
        up.cross(viewDirection, &lookAtRightVector);
        if (lookAtRightVector.lengthSquared() < c::floatEpsilon)
        {
            // we're pointing up (or down), use fwd as up vector
            fwd.cross(viewDirection, &lookAtRightVector);
        }

        lookAtRightVector.normalize();
        viewDirection.cross(lookAtRightVector, &lookAtUpVector);

        lookAtUpVector.normalize();

        matrix44 tmp;
        tmp.m[0][0] = lookAtRightVector.x;
        tmp.m[0][1] = lookAtUpVector.x;
        tmp.m[0][2] = viewDirection.x;
        tmp.m[0][3] = eye.x;

        tmp.m[1][0] = lookAtRightVector.y;
        tmp.m[1][1] = lookAtUpVector.y;
        tmp.m[1][2] = viewDirection.y;
        tmp.m[1][3] = eye.y;

        tmp.m[2][0] = lookAtRightVector.z;
        tmp.m[2][1] = lookAtUpVector.z;
        tmp.m[2][2] = viewDirection.z;
        tmp.m[2][3] = eye.z;

        tmp.m[3][0] = 0.0f;
        tmp.m[3][1] = 0.0f;
        tmp.m[3][2] = 0.0f;
        tmp.m[3][3] = 1.0f;

        // we will compute camera to world
        // tmp is in canonical coord system
        // move it into cam
        matrix44 worldToCanonicalCam;
        tmp.apply(invCoords, &worldToCanonicalCam);

        // now go frame cam space in canonical into cam space in world
        coords.apply(worldToCanonicalCam, (matrix44*)this->m);
    }

    BAJA_INLINE void matrix44::makeOrthographicProjection(
        float32 left, 
        float32 right, 
        float32 top, 
        float32 bottom, 
        float32 nearZ, 
        float32 farZ
        )
    {
        float32 width = right - left;
        float32 height = top - bottom;
        float32 depth = farZ - nearZ;

        this->m[0][0] = 2.0f/width;
        this->m[0][1] = 0.0f;
        this->m[0][2] = 0.0f;
        this->m[0][3] = -(left+right)/width;

        this->m[1][0] = 0.0f;
        this->m[1][1] = 2.0f/height;
        this->m[1][2] = 0.0f;
        this->m[1][3] = -(top+bottom)/height;

        this->m[2][0] = 0.0f;
        this->m[2][1] = 0.0f;
        this->m[2][2] = 1.0f/depth;
        this->m[2][3] = -nearZ/depth;

        this->m[3][0] = 0.0f;
        this->m[3][1] = 0.0f;
        this->m[3][2] = 0.0f;
        this->m[3][3] = 1.0f;
    }

    BAJA_INLINE void matrix44::makePerspectiveProjection(
        float32 widthAtFocalPoint, 
        float32 heightAtFocalPoint, 
        float32 nearZ, 
        float32 focalPointZ, 
        float32 farZ
        )
    {
        this->makePerspectiveProjectionLH(
            widthAtFocalPoint,
            heightAtFocalPoint,
            nearZ,
            focalPointZ,
            farZ
            );
    }

    BAJA_INLINE void matrix44::makePerspectiveProjectionLH(
        float32 widthAtFocalPoint, 
        float32 heightAtFocalPoint, 
        float32 nearZ, 
        float32 focalPointZ, 
        float32 farZ
        )
    {
        BAJA_VERIFY(widthAtFocalPoint > 0.0f);
        BAJA_VERIFY(heightAtFocalPoint > 0.0f);
        BAJA_VERIFY(nearZ > 0.0f);
        BAJA_VERIFY(focalPointZ > nearZ);
        BAJA_VERIFY(farZ > nearZ);

        // calculate width and height at near plane
        float32 widthNear = 2.0f * (nearZ * (widthAtFocalPoint / 2.0f) / focalPointZ);
        float32 heightNear = 2.0f * (nearZ * (heightAtFocalPoint / 2.0f) / focalPointZ);

        // initialize the matrix
        this->m[0][0] = 2.0f * nearZ / widthNear;
        this->m[0][1] = 0.0f;
        this->m[0][2] = 0.0f;
        this->m[0][3] = 0.0f;

        this->m[1][0] = 0.0f;
        this->m[1][1] = 2.0f * nearZ / heightNear;
        this->m[1][2] = 0.0f;
        this->m[1][3] = 0.0f;

        this->m[2][0] = 0.0f;
        this->m[2][1] = 0.0f;
        this->m[2][2] = farZ / (farZ - nearZ);
        this->m[2][3] = nearZ * farZ / (nearZ - farZ);

        this->m[3][0] = 0.0f;
        this->m[3][1] = 0.0f;
        this->m[3][2] = 1.0f;
        this->m[3][3] = 0.0f;
    }

    BAJA_INLINE void matrix44::makePerspectiveProjection(
        float32 leftAtNear, 
        float32 rightAtNear, 
        float32 topAtNear, 
        float32 bottomAtNear,
        float32 nearZ,
        float32 farZ
        )
    {
        BAJA_VERIFY(nearZ > 0.0f);
        BAJA_VERIFY(farZ > nearZ);

        float32 deltaRL = rightAtNear - leftAtNear;
        if (deltaRL == 0.0f)
        {
            deltaRL = c::floatEpsilon;
        }
        BAJA_VERIFY(deltaRL != 0.0f);

        float32 deltaTB = topAtNear - bottomAtNear;
        if (deltaTB == 0.0f)
        {
            deltaTB = c::floatEpsilon;
        }
        BAJA_VERIFY(deltaTB != 0.0f);

        float32 sumRL = rightAtNear + leftAtNear;
        float32 sumTB = topAtNear + bottomAtNear;

        float32 oneOverDeltaRL = 1 / deltaRL;
        float32 oneOverDeltaTB = 1 / deltaTB;

        float32 deltaNF = nearZ - farZ;
        float32 oneOverDeltaNF = 1 / deltaNF;

        this->m[0][0] = 2 * nearZ * oneOverDeltaRL;
        this->m[0][1] = 0;
        this->m[0][2] = -sumRL * oneOverDeltaRL;
        this->m[0][3] = 0;

        this->m[1][0] = 0;
        this->m[1][1] = 2 * nearZ * oneOverDeltaTB;
        this->m[1][2] = -sumTB * oneOverDeltaTB;
        this->m[1][3] = 0;

        this->m[2][0] = 0;
        this->m[2][1] = 0;
        this->m[2][2] = -(farZ) * oneOverDeltaNF;
        this->m[2][3] = nearZ * farZ * oneOverDeltaNF;

        this->m[3][0] = 0;
        this->m[3][1] = 0;
        this->m[3][2] = 1;
        this->m[3][3] = 0;
    }

    BAJA_INLINE void matrix44::invert()
    {
        matrix44 t;

        this->getInverse(&t);

        *this = t;
    }

    BAJA_INLINE void matrix44::normalize()
    {
        float32 magnitudeSq;
        float32 magnitude;
        float32 magnitudeInv;
        uint32 i;

        for (i = 0; i < 3; i++)
        {
            magnitudeSq = this->m[0][i] * this->m[0][i] + this->m[1][i] * this->m[1][i] + this->m[2][i] * this->m[2][i];
            magnitude = sqrtf(magnitudeSq);

            magnitudeInv = (fabs(magnitude) > c::floatEpsilon) ? 1.0f / magnitude : 0.0f;

            this->m[0][i] *= magnitudeInv;
            this->m[1][i] *= magnitudeInv;
            this->m[2][i] *= magnitudeInv;
        }
    }

    BAJA_INLINE void matrix44::scale(const float32 scale)
    { 
        this->scale(scale, scale, scale); 
    }

    BAJA_INLINE void matrix44::scale(float32 xScale, float32 yScale, float32 zScale)
    {
        matrix44 t(*this);
        matrix44 s;

        s.setScale(xScale, yScale, zScale);
        s.apply(t, this);
    }

    BAJA_INLINE void matrix44::translate(const vector3d& translation)
    {
        this->m[0][3] += translation.x;
        this->m[1][3] += translation.y;
        this->m[2][3] += translation.z;
    }

    BAJA_INLINE bool
        isOBBIntersectingLineSegment(
        const point3d& boxCenter,
        const vector3d& boxExtents0,
        const vector3d& boxExtents1,
        const vector3d& boxExtents2,
        const line3d& lineSegment
        )
    {
        point3d midPoint;
        lineSegment.getPointAt(0.5f, &midPoint);

        vector3d V = lineSegment.direction;
        V.scale(0.5f);

        vector3d D;
        boxCenter.vectorTo(midPoint, &D);

        vector3d U[3];
        boxExtents0.normalize(&U[0]);
        boxExtents1.normalize(&U[1]);
        boxExtents2.normalize(&U[2]);

        float32 e[3];
        e[0] = boxExtents0.length();
        e[1] = boxExtents1.length();
        e[2] = boxExtents2.length();

        for (uint32 i = 0; i < 3; i++)
        {
            float32 Rd = fabsf(D.dot(U[i]));
            if (Rd > e[i] + fabsf(V.dot(U[i])))
            {
                return false;
            }
        }

        float32 VU[3];
        VU[0] = fabsf(V.dot(U[0]));
        VU[1] = fabsf(V.dot(U[1]));
        VU[2] = fabsf(V.dot(U[2]));

        vector3d crossVD;
        V.cross(D, &crossVD);

        if (fabsf(crossVD.dot(U[0])) > (e[1] * VU[2] + e[2] * VU[1]))
        {
            return false;
        }
        if (fabsf(crossVD.dot(U[1])) > (e[0] * VU[2] + e[2] * VU[0]))
        {
            return false;
        }
        if (fabsf(crossVD.dot(U[2])) > (e[0] * VU[1] + e[1] * VU[0]))
        {
            return false;
        }

        return true;
    }

    BAJA_INLINE bool 
        isOBBIntersectingTriangle(
        const point3d& boxCenter,
        const vector3d& boxExtents0,
        const vector3d& boxExtents1,
        const vector3d& boxExtents2,
        const point3d& triPoint0,
        const point3d& triPoint1,
        const point3d& triPoint2
        )
    {
        vector3d e[3];
        triPoint0.vectorTo(triPoint1, &e[0]);
        triPoint0.vectorTo(triPoint2, &e[1]);
        triPoint1.vectorTo(triPoint2, &e[2]);

        vector3d triNormal;
        e[0].cross(e[1], &triNormal);

        vector3d D;
        boxCenter.vectorTo(triPoint0, &D);

        vector3d A[3];
        boxExtents0.normalize(&A[0]);
        boxExtents1.normalize(&A[1]);
        boxExtents2.normalize(&A[2]);

        float32 a[3];
        a[0] = boxExtents0.length();
        a[1] = boxExtents1.length();
        a[2] = boxExtents2.length();

        float32 R = a[0] * fabsf(triNormal.dot(A[0])) + a[1] * fabsf(triNormal.dot(A[1])) + a[2] * fabsf(triNormal.dot(A[2]));
        float32 ndotD = triNormal.dot(D);
        if (fabsf(ndotD) > R)
        {
            return false;
        }

        float32 p[3];

        for (uint32 i = 0; i < 3; i++)
        {
            p[0] = A[i].dot(D);
            p[1] = p[0] + A[i].dot(e[0]);
            p[2] = p[0] + A[i].dot(e[1]);
            R = a[i];

            if (p[0] > R)
            {
                if (p[1] > R && p[2] > R)
                {
                    return false;
                }
            }
            else if (p[0] < -R)
            {
                if (p[1] < -R && p[2] < -R)
                {
                    return false;
                }
            }
        }

        //
        // 9 more tests that correspond to Ai X Ej axes
        //
        float32 p0AETests[9];
        for (uint32 i = 0; i < 3; i++)
        {
            for (uint32 j = 0; j < 3; j++)
            {
                vector3d crossAE;
                A[i].cross(e[j], &crossAE);

                p0AETests[i*3 + j] = crossAE.dot(D); 
            }
        }

        float32 AN[3];
        AN[0] = A[0].dot(triNormal);
        AN[1] = A[1].dot(triNormal);
        AN[2] = A[2].dot(triNormal);

        float32 p1AETests[9];
        for (uint32 i = 0; i < 3; i++)
        {
            for (uint32 j = 0; j < 3; j++)
            {
                uint32 test = i*3 + j;
                float32 p1 = p0AETests[test];

                if (j != 0)
                {
                    p1 -= AN[i];
                }

                p1AETests[test] = p1;
            }
        }

        float32 p2Table[3] = {1, 0, -1};

        float32 p2AETests[9];
        for (uint32 i = 0; i < 3; i++)
        {
            for (uint32 j = 0; j < 3; j++)
            {
                uint32 test = i*3 + j;

                p2AETests[test] = p0AETests[test] + p2Table[j] * AN[i];
            }
        }

        float32 AiEj[3][3];
        AiEj[0][0] = fabsf(A[0].dot(e[0]));
        AiEj[0][1] = fabsf(A[0].dot(e[1]));
        AiEj[0][2] = fabsf(A[0].dot(e[2]));
        AiEj[1][0] = fabsf(A[1].dot(e[0]));
        AiEj[1][1] = fabsf(A[1].dot(e[1]));
        AiEj[1][2] = fabsf(A[1].dot(e[2]));
        AiEj[2][0] = fabsf(A[2].dot(e[0]));
        AiEj[2][1] = fabsf(A[2].dot(e[1]));
        AiEj[2][2] = fabsf(A[2].dot(e[2]));

#define RAiEj(x0, x1, x2, x3, x4, x5) a[x0] * AiEj[x1][x2] + a[x3] * AiEj[x4][x5]

        float32 RAETests[9];
        RAETests[0] = RAiEj(1, 2, 0, 2, 1, 0);
        RAETests[1] = RAiEj(1, 2, 1, 2, 1, 1);
        RAETests[2] = RAiEj(1, 2, 2, 2, 1, 2);
        RAETests[3] = RAiEj(0, 2, 0, 2, 0, 0);
        RAETests[4] = RAiEj(0, 2, 1, 2, 0, 1);
        RAETests[5] = RAiEj(0, 2, 2, 2, 0, 2);
        RAETests[6] = RAiEj(0, 1, 0, 1, 0, 0);
        RAETests[7] = RAiEj(0, 1, 1, 1, 0, 1);
        RAETests[8] = RAiEj(0, 1, 2, 1, 0, 2);

        for (uint32 i = 0; i < 9; i++)
        {
            p[0] = p0AETests[i];
            p[1] = p1AETests[i];
            p[2] = p2AETests[i];
            R = RAETests[i];

            if (p[0] > R)
            {
                if (p[1] > R && p[2] > R)
                {
                    return false;
                }
            }
            else if (p[0] < -R)
            {
                if (p[1] < -R && p[2] < -R)
                {
                    return false;
                }
            }
        }

        return true;
    }

    // result = scale0 * v0 + scale1 * v1
    BAJA_INLINE void
        linearCombine(
        const vector3d& v0, 
        float32 scale0, 
        const vector3d& v1, 
        float32 scale1, 
        vector3d* outResult
        )
    {
        if (outResult)
        {
            outResult->x = scale0 * v0.x + scale1 * v1.x;
            outResult->y = scale0 * v0.y + scale1 * v1.y;
            outResult->z = scale0 * v0.z + scale1 * v1.z;
        }
    }

}} // namespace baja.math
